#include "BEMUtil.h"
#include <stdio.h>

extern double shiftAlpha, shearModulus, poissonRatio, Pi;

Mat33d evaluateQuadraturePoint( Vec3d p, Vec3d q )
{
	Vec3d r = p - q;

	double rn = mag(r);

	if (rn < shiftAlpha)
	{
		rn = shiftAlpha;
	}

	Vec3d rg = r / rn;

	double Me[9];
	double Ie[9];

	Me[0] = rg[0] * rg[0];
	Me[1] = rg[0] * rg[1];
	Me[2] = rg[0] * rg[2];

	Me[3] = rg[1] * rg[0];
	Me[4] = rg[1] * rg[1];
	Me[5] = rg[1] * rg[2];

	Me[6] = rg[2] * rg[0];
	Me[7] = rg[2] * rg[1];
	Me[8] = rg[2] * rg[2];

	Ie[0] = 1;
	Ie[1] = 0;
	Ie[2] = 0;

	Ie[3] = 0;
	Ie[4] = 1;
	Ie[5] = 0;

	Ie[6] = 0;
	Ie[7] = 0;
	Ie[8] = 1;

	Mat33d M(Me);
	Mat33d I(Ie);

	Mat33d S;

	S = 1.0 / (16.0 * Pi * (1.0 - poissonRatio) * shearModulus * (rn + shiftAlpha)) * ((3 - 4 * poissonRatio) * I + M);

	return S;
}

Mat33d tensorVectorDot( double A[3][3][3], Vec3d v )
{

	double B[9][3];

	B[0][0] = A[0][0][0];
	B[1][0] = A[1][0][0];
	B[2][0] = A[2][0][0];
	B[3][0] = A[0][1][0];
	B[4][0] = A[1][1][0];
	B[5][0] = A[2][1][0];
	B[6][0] = A[0][2][0];
	B[7][0] = A[1][2][0];
	B[8][0] = A[2][2][0];

	B[0][1] = A[0][0][1];
	B[1][1] = A[1][0][1];
	B[2][1] = A[2][0][1];
	B[3][1] = A[0][1][1];
	B[4][1] = A[1][1][1];
	B[5][1] = A[2][1][1];
	B[6][1] = A[0][2][1];
	B[7][1] = A[1][2][1];
	B[8][1] = A[2][2][1];

	B[0][2] = A[0][0][2];
	B[1][2] = A[1][0][2];
	B[2][2] = A[2][0][2];
	B[3][2] = A[0][1][2];
	B[4][2] = A[1][1][2];
	B[5][2] = A[2][1][2];
	B[6][2] = A[0][2][2];
	B[7][2] = A[1][2][2];
	B[8][2] = A[2][2][2];

	double C[9];

	for (int i=0; i<9; i++)
	{
		C[i] = 0;

		for (int j=0; j<3; j++)
		{
			C[i] += B[i][j] * v[j];
		}
	}

	Mat33d MT(C);

	return MT;
}


Mat33d tensorVectorContraction( double A[3][3][3], Vec3d v )
{
	double B[3][9];

	B[0][0] = A[0][0][0];
	B[1][0] = A[0][1][0];
	B[2][0] = A[0][2][0];
	B[0][1] = A[1][0][0];
	B[1][1] = A[1][1][0];
	B[2][1] = A[1][2][0];
	B[0][2] = A[2][0][0];
	B[1][2] = A[2][1][0];
	B[2][2] = A[2][2][0];

	B[0][3] = A[0][0][1];
	B[1][3] = A[0][1][1];
	B[2][3] = A[0][2][1];
	B[0][4] = A[1][0][1];
	B[1][4] = A[1][1][1];
	B[2][4] = A[1][2][1];
	B[0][5] = A[2][0][1];
	B[1][5] = A[2][1][1];
	B[2][5] = A[2][2][1];

	B[0][6] = A[0][0][2];
	B[1][6] = A[0][1][2];
	B[2][6] = A[0][2][2];
	B[0][7] = A[1][0][2];
	B[1][7] = A[1][1][2];
	B[2][7] = A[1][2][2];
	B[0][8] = A[2][0][2];
	B[1][8] = A[2][1][2];
	B[2][8] = A[2][2][2];

	double C[9];

	for (int i=0; i<9; i++)
	{
		C[i] = 0;

		for (int j=0; j<3; j++)
		{
			C[i] += B[j][i] * v[j];
		}
	}

	Mat33d MT(C);

	return MT;

}


double* tensorProductGradient( double s, Vec3d ds, Mat33d A, double dA[3][3][3] )
{
	double K[3][9];

	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			for (int k=0; k<3; k++)
			{
				K[j][k+i*3] = A(j, k) * ds[i];
			}
		}
	}

	double dsA[3][3][3];
	double* M = (double*)malloc(27 * sizeof(double));

	dsA[0][0][0] = K[0][0];
	dsA[0][1][0] = K[0][1];
	dsA[0][2][0] = K[0][2];
	dsA[1][0][0] = K[1][0];
	dsA[1][1][0] = K[1][1];
	dsA[1][2][0] = K[1][2];
	dsA[2][0][0] = K[2][0];
	dsA[2][1][0] = K[2][1];
	dsA[2][2][0] = K[2][2];

	dsA[0][0][1] = K[0][3];
	dsA[0][1][1] = K[0][4];
	dsA[0][2][1] = K[0][5];
	dsA[1][0][1] = K[1][3];
	dsA[1][1][1] = K[1][4];
	dsA[1][2][1] = K[1][5];
	dsA[2][0][1] = K[2][3];
	dsA[2][1][1] = K[2][4];
	dsA[2][2][1] = K[2][5];

	dsA[0][0][2] = K[0][6];
	dsA[0][1][2] = K[0][7];
	dsA[0][2][2] = K[0][8];
	dsA[1][0][2] = K[1][6];
	dsA[1][1][2] = K[1][7];
	dsA[1][2][2] = K[1][8];
	dsA[2][0][2] = K[2][6];
	dsA[2][1][2] = K[2][7];
	dsA[2][2][2] = K[2][8];

	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			for (int k=0; k<3; k++)
			{
				M[getIndex(i,j,k)] = dsA[i][j][k] + s * dA[i][j][k];
			}
		}
	}

	return M;
}


int getIndex( int i, int j, int k )
{
	return i * 9 + j * 3 + k;
}


double* getDrM( Vec3d r, double rn, Mat33d M )
{
	double down = rn * rn * rn * rn;

	double s = 1.0 / rn;
	Mat33d A = M;
	Vec3d ds = r / (rn * rn * rn) * (-1.0);

	double dAm[3][9];

	dAm[0][0] = 2 * r[0] * (r[1] * r[1] + r[2] * r[2]) / down;
	dAm[0][1] = -1 * r[1] * (r[0] * r[0] - r[2] * r[2] - r[1] * r[1]) / down;
	dAm[0][2] = -1 * r[2] * (r[0] * r[0] - r[2] * r[2] - r[1] * r[1]) / down;
	dAm[0][3] = -2 * r[1] * r[0] * r[0] / down;
	dAm[0][4] = -1 * r[0] * (r[1] * r[1] - r[2] * r[2] - r[0] * r[0]) / down;
	dAm[0][5] = -2 * r[1] * r[2] * r[0] / down;
	dAm[0][6] = -2 * r[0] * r[2] * r[0] / down;
	dAm[0][7] = -2 * r[1] * r[2] * r[0] / down;
	dAm[0][8] = -1 * r[0] * (r[2] * r[2] - r[1] * r[1] - r[0] * r[0]) / down;

	dAm[1][0] = -1 * r[1] * (r[0] * r[0] - r[2] * r[2] - r[1] * r[1]) / down;
	dAm[1][1] = -2 * r[0] * r[1] * r[1] / down;
	dAm[1][2] = -2 * r[1] * r[2] * r[0] / down;
	dAm[1][3] = -1 * r[0] * (r[1] * r[1] - r[2] * r[2] - r[0] * r[0]) / down;
	dAm[1][4] = 2 * r[1] * (r[0] * r[0] + r[2] * r[2]) / down;
	dAm[1][5] = -1 * r[2] * (r[1] * r[1] - r[2] * r[2] - r[0] * r[0]) / down;
	dAm[1][6] = -2 * r[1] * r[2] * r[0] / down;
	dAm[1][7] = -2 * r[1] * r[2] * r[1] / down;
	dAm[1][8] = -1 * r[1] * (r[2] * r[2] - r[0] * r[0] - r[1] * r[1]) / down;

	dAm[2][0] = -1 * r[2] * (r[0] * r[0] - r[2] * r[2] - r[1] * r[1]) / down;
	dAm[2][1] = -2 * r[1] * r[2] * r[0] / down;
	dAm[2][2] = -2 * r[2] * r[2] * r[0] / down;
	dAm[2][3] = -2 * r[1] * r[2] * r[0] / down;
	dAm[2][4] = -1 * r[2] * (r[1] * r[1] - r[2] * r[2] - r[0] * r[0]) / down;
	dAm[2][5] = -2 * r[1] * r[2] * r[2] / down;
	dAm[2][6] = -1 * r[0] * (r[2] * r[2] - r[0] * r[0] - r[1] * r[1]) / down;
	dAm[2][7] = -1 * r[1] * (r[2] * r[2] - r[0] * r[0] - r[1] * r[1]) / down;
	dAm[2][8] = 2 * r[2] * (r[0] * r[0] + r[1] * r[1]) / down;


	double dA[3][3][3];


	dA[0][0][0] = dAm[0][0];
	dA[0][1][0] = dAm[0][1];
	dA[0][2][0] = dAm[0][2];
	dA[1][0][0] = dAm[1][0];
	dA[1][1][0] = dAm[1][1];
	dA[1][2][0] = dAm[1][2];
	dA[2][0][0] = dAm[2][0];
	dA[2][1][0] = dAm[2][1];
	dA[2][2][0] = dAm[2][2];

	dA[0][0][1] = dAm[0][3];
	dA[0][1][1] = dAm[0][4];
	dA[0][2][1] = dAm[0][5];
	dA[1][0][1] = dAm[1][3];
	dA[1][1][1] = dAm[1][4];
	dA[1][2][1] = dAm[1][5];
	dA[2][0][1] = dAm[2][3];
	dA[2][1][1] = dAm[2][4];
	dA[2][2][1] = dAm[2][5];

	dA[0][0][2] = dAm[0][6];
	dA[0][1][2] = dAm[0][7];
	dA[0][2][2] = dAm[0][8];
	dA[1][0][2] = dAm[1][6];
	dA[1][1][2] = dAm[1][7];
	dA[1][2][2] = dAm[1][8];
	dA[2][0][2] = dAm[2][6];
	dA[2][1][2] = dAm[2][7];
	dA[2][2][2] = dAm[2][8];

	return tensorProductGradient(s, ds, A, dA);
}


double* getDrI( Vec3d r, double rn, Mat33d I )
{
	double s = 1.0 / rn;
	Mat33d A = I;
	Vec3d ds = r / (rn * rn * rn) * (-1.0);

	double dA[3][3][3];

	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			for (int k=0; k<3; k++)
			{
				dA[i][j][k] = 0;
			}
		}
	}

	return tensorProductGradient(s, ds, A, dA);
}



double* getKernelGradient( Vec3d p, Vec3d q )
{
	Vec3d r = p - q;

	double rn = mag(r);

	if (rn < shiftAlpha)
	{
		rn = shiftAlpha;
	}

	Vec3d rg = r / rn;

	double Me[9];
	double Ie[9];

	Me[0] = rg[0] * rg[0];
	Me[1] = rg[0] * rg[1];
	Me[2] = rg[0] * rg[2];

	Me[3] = rg[1] * rg[0];
	Me[4] = rg[1] * rg[1];
	Me[5] = rg[1] * rg[2];

	Me[6] = rg[2] * rg[0];
	Me[7] = rg[2] * rg[1];
	Me[8] = rg[2] * rg[2];

	Ie[0] = 1;
	Ie[1] = 0;
	Ie[2] = 0;

	Ie[3] = 0;
	Ie[4] = 1;
	Ie[5] = 0;

	Ie[6] = 0;
	Ie[7] = 0;
	Ie[8] = 1;

	Mat33d M(Me);
	Mat33d I(Ie);

	double* DrM = getDrM(r, rn, M);
	double* DrI = getDrI(r, rn, I);

	double* Z = (double*)malloc(27 * sizeof(double));

	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			for (int k=0; k<3; k++)
			{
				Z[getIndex(i,j,k)] = ((3.0 - 4.0 * poissonRatio) * DrI[getIndex(i,j,k)] + DrM[getIndex(i,j,k)]) / (16.0 * Pi * shearModulus * (1.0 - poissonRatio));
			}
		}
	}

	free(DrM);
	free(DrI);

	return Z;
}



Mat33d evaluateGradientQuadraturePoint( Vec3d p, Vec3d q, Vec3d qn )
{
	double* Kd = getKernelGradient(p, q);

	double K[3][3][3];

	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			for (int k=0; k<3; k++)
			{
				K[i][j][k] = Kd[getIndex(i,j,k)];				
			}
		}
	}

	free(Kd);

	return tensorVectorDot(K, qn);
}

int getTensorArrayIndex( int i, int j, int k )
{
	return k * 9 + i * 3 + j;
}

